
#include "ChaosMath.hpp"
#include "ChaosMatrix.hpp"
#include "ChaosQuaternion.hpp"
#include <math.h>

namespace Chaos
{

	ChsMatrix& MatrixRotationQuaternion ( ChsMatrix & pOut, const ChsQuaternion & pQ)
	{
		pOut._m11 = 1-2*( pQ.m_fY * pQ.m_fY + pQ.m_fZ * pQ.m_fZ);
		pOut._m12 = 2*(pQ.m_fX * pQ.m_fY + pQ.m_fW * pQ.m_fZ);
		pOut._m13 = 2*(pQ.m_fX * pQ.m_fZ - pQ.m_fW * pQ.m_fY);
		pOut._m14 = 0;
		
		pOut._m21 = 2*(pQ.m_fX * pQ.m_fY - pQ.m_fW * pQ.m_fZ);
		pOut._m22 = 1- 2*( pQ.m_fX * pQ.m_fX + pQ.m_fZ * pQ.m_fZ);
		pOut._m23 = 2*(pQ.m_fZ * pQ.m_fY + pQ.m_fW * pQ.m_fX);
		pOut._m24 = 0;

		pOut._m31 = 2*(pQ.m_fX * pQ.m_fZ + pQ.m_fW * pQ.m_fY);
		pOut._m32 = 2*( pQ.m_fY * pQ.m_fZ - pQ.m_fW * pQ.m_fX);
		pOut._m33 = 1- 2*( pQ.m_fX * pQ.m_fX + pQ.m_fY * pQ.m_fY);
		pOut._m34 = 0;

		pOut._m41 = 0;
		pOut._m42 = 0;
		pOut._m43 = 0;
		pOut._m44 = 1;

		return pOut;
	}

	ChsQuaternion& QuaternionRotationMatrix ( ChsQuaternion & pQ ,const ChsMatrix & pM)
	{
		float m11 = pM._m11,	m12 = pM._m12,	m13 = pM._m13;
		float m21 = pM._m21,	m22 = pM._m22,	m23 = pM._m23;
		float m31 = pM._m31,	m32 = pM._m32,	m33 = pM._m33;

		float w=0.0f,x=0.0f,y=0.0f,z=0.0f;

		float fourWSquaredMinusl = m11+m22+m33;
		float fourXSquaredMinusl = m11-m22-m33;
		float fourYSquaredMinusl = m22-m11-m33;
		float fourZSquaredMinusl = m33-m11-m22;

		int biggestIndex = 0;

		float fourBiggestSquaredMinusl = fourWSquaredMinusl;

		if(fourXSquaredMinusl>fourBiggestSquaredMinusl)
		{
			fourBiggestSquaredMinusl = fourXSquaredMinusl;
			biggestIndex = 1;
		}
		if(fourYSquaredMinusl>fourBiggestSquaredMinusl)
		{
			fourBiggestSquaredMinusl = fourYSquaredMinusl;
			biggestIndex = 2;
		}
		if(fourZSquaredMinusl>fourBiggestSquaredMinusl)
		{
			fourBiggestSquaredMinusl = fourZSquaredMinusl;
			biggestIndex = 3;
		}

		float biggestVal = sqrt(fourBiggestSquaredMinusl + 1.0f)*0.5f;
		float mult = 0.25f /biggestVal;

		switch(biggestIndex)
		{
		case 0:
			w = biggestVal;
			x = (m23-m32) * mult;
			y = (m31-m13) * mult;
			z = (m12-m21) * mult;
			break;
		case 1:
			x = biggestVal;
			w = (m23-m32) * mult;
			y = (m12+m21) * mult;
			z = (m31+m13) * mult;
			break;
		case 2:
			y = biggestVal;
			w = (m31-m13) * mult;
			x = (m12+m21) * mult;
			z = (m23+m32) * mult;
			break;
		case 3:
			z = biggestVal;
			w = (m12-m21) * mult;
			x = (m31+m13) * mult;
			y = (m23+m32) * mult;
			break;
		}
		pQ.m_fW = w;
		pQ.m_fX = x;
		pQ.m_fY = y;
		pQ.m_fZ = z;
		return pQ;
	}


}